/*
 * This file is part of the OpenMV project.
 *
 * Copyright (c) 2013-2021 Ibrahim Abdelkader <iabdalkader@openmv.io>
 * Copyright (c) 2013-2021 Kwabena W. Agyeman <kwagyeman@openmv.io>
 *
 * This work is licensed under the MIT license, see the file LICENSE for details.
 *
 * MT9M114 driver.
 */
#include "omv_boardconfig.h"
#if (OMV_ENABLE_MT9M114 == 1)

#include <stdint.h>
#include <stdlib.h>
#include <string.h>

#include "cambus.h"
#include "sensor.h"
#include "mt9m114.h"
#include "py/mphal.h"

#define CMD_TIMEOUT                              (1000)

#define REG_CHIP_VERSION_REG                    (0x0000)
#define REG_CLOCKS_CONTROL                      (0x0016)
#define REG_RESET_AND_MISC_CONTROL              (0x001A)
#define REG_PAD_SLEW                            (0x001E)
#define REG_USER_DEFINED_DEVICE_ADDRESS_ID      (0x002E)
#define REG_PAD_CONTROL                         (0x0032)
#define REG_HOST_COMMAND                        (0x0080)
#define HC_APPLY_PATCH                          (0x0001)
#define HC_SET_STATE                            (0x0002)
#define HC_REFRESH                              (0x0004)
#define HC_WAIT_FOR_EVENT                       (0x0008)
#define HC_OK                                   (0x8000)

#define REG_Y_ADDR_START                        (0x3002)
#define REG_X_ADDR_START                        (0x3004)
#define REG_Y_ADDR_END                          (0x3006)
#define REG_X_ADDR_END                          (0x3008)
#define REG_FRAME_LENGTH_LINES                  (0x300A)
#define REG_LINE_LENGTH_PCK                     (0x300C)
#define REG_COARSE_INTEGRATION_TIME             (0x3012)
#define REG_FINE_INTEGRATION_TIME               (0x3014)
#define REG_RESET_REGISTER                      (0x301A)
#define REG_FLASH                               (0x3046)
#define REG_FLASH_COUNT                         (0x3048)
#define REG_GREEN1_GAIN                         (0x3056)
#define REG_BLUE_GAIN                           (0x3058)
#define REG_RED_GAIN                            (0x305A)
#define REG_GREEN2_GAIN                         (0x305C)
#define REG_GLOBAL_GAIN_REG                     (0x305E)

#define REG_SYSMGR_NEXT_STATE                   (0xDC00)
#define REG_SYSMGR_CURRENT_STATE                (0xDC01)
#define REG_SYSMGR_CMD_STATUS                   (0xDC02)
#define SS_ENTER_CONFIG_CHANGE                  (  0x28) 
#define SS_STREAMING                            (  0x31) 
#define SS_START_STREAMING                      (  0x34) 
#define SS_ENTER_SUSPEND                        (  0x40) 
#define SS_SUSPENDED                            (  0x41) 
#define SS_ENTER_STANDBY                        (  0x50) 
#define SS_STANDBY                              (  0x52) 
#define SS_LEAVE_STANDBY                        (  0x54) 

#define REG_CAM_OUTPUT_FORMAT                   (0xC86C)
#define OUTPUT_FORMAT_RGB565                    (0 << 12)
#define OUTPUT_FORMAT_RGB555                    (1 << 12)
#define OUTPUT_FORMAT_xRGB444                   (2 << 12)
#define OUTPUT_FORMAT_RGB444x                   (3 << 12)
#define OUTPUT_FORMAT_YUV                       (0 << 8)
#define OUTPUT_FORMAT_RGB                       (1 << 8)
#define OUTPUT_FORMAT_BAYER                     (2 << 8)
#define OUTPUT_FORMAT_FVLV_DISABLE              (1 << 5)
#define OUTPUT_FORMAT_BT656_CROP_DISABLE        (1 << 4)
#define OUTPUT_FORMAT_BT656                     (1 << 3)
#define OUTPUT_FORMAT_MONO                      (1 << 2)
#define OUTPUT_FORMAT_SWAP_BYTES                (1 << 1)
#define OUTPUT_FORMAT_SWAP_RB                   (1 << 0)

static const uint32_t qvga_24mhz_regs[][3] = {
    // Generated by: MT9M114 (SOC1040) Register Wizard
    { 0x98E,    2, 0x1000},
    { 0xC97E,   1, 0x01  },        // cam_sysctl_pll_enable = 1
    { 0xC980,   2, 0x0120},        // cam_sysctl_pll_divider_m_n = 288
    { 0xC982,   2, 0x0700},        // cam_sysctl_pll_divider_p = 1792
    { 0xC984,   2, 0x8000},        // cam_port_output_control = 32768
    { 0xC800,   2, 0x0000},        // cam_sensor_cfg_y_addr_start = 0
    { 0xC802,   2, 0x0000},        // cam_sensor_cfg_x_addr_start = 0
    { 0xC804,   2, 0x03CD},        // cam_sensor_cfg_y_addr_end = 973
    { 0xC806,   2, 0x050D},        // cam_sensor_cfg_x_addr_end = 1293
    { 0xC808,   4, 0x2DC6C00},     // cam_sensor_cfg_pixclk = 48000000
    { 0xC80C,   2, 0x0001},        // cam_sensor_cfg_row_speed = 1
    { 0xC80E,   2, 0x01C3},        // cam_sensor_cfg_fine_integ_time_min = 451
    { 0xC810,   2, 0x03BA},        // cam_sensor_cfg_fine_integ_time_max = 954
    { 0xC812,   2, 0x02DE},        // cam_sensor_cfg_frame_length_lines = 734
    { 0xC814,   2, 0x04A5},        // cam_sensor_cfg_line_length_pck = 1189
    { 0xC816,   2, 0x00E0},        // cam_sensor_cfg_fine_correction = 224
    { 0xC818,   2, 0x01E3},        // cam_sensor_cfg_cpipe_last_row = 483
    { 0xC826,   2, 0x0020},        // cam_sensor_cfg_reg_0_data = 32
    { 0xC834,   2, 0x0330},        // cam_sensor_control_read_mode = 816
    { 0xC854,   2, 0x0000},        // cam_crop_window_xoffset = 0
    { 0xC856,   2, 0x0000},        // cam_crop_window_yoffset = 0
    { 0xC858,   2, 0x0280},        // cam_crop_window_width = 640
    { 0xC85A,   2, 0x01E0},        // cam_crop_window_height = 480
    { 0xC85C,   1, 0x03  },        // cam_crop_cropmode = 3
    { 0xC868,   2, 0x0140},        // cam_output_width = 320
    { 0xC86A,   2, 0x00F0},        // cam_output_height = 240
    { 0xC878,   1, 0x00  },        // cam_aet_aemode = 0
    { 0xC88C,   2, 0x3700},        // cam_aet_max_frame_rate = 14080
    { 0xC88E,   2, 0x3700},        // cam_aet_min_frame_rate = 14080
    { 0xC914,   2, 0x0000},        // cam_stat_awb_clip_window_xstart = 0
    { 0xC916,   2, 0x0000},        // cam_stat_awb_clip_window_ystart = 0
    { 0xC918,   2, 0x013F},        // cam_stat_awb_clip_window_xend = 319
    { 0xC91A,   2, 0x00EF},        // cam_stat_awb_clip_window_yend = 239
    { 0xC91C,   2, 0x0000},        // cam_stat_ae_initial_window_xstart = 0
    { 0xC91E,   2, 0x0000},        // cam_stat_ae_initial_window_ystart = 0
    { 0xC920,   2, 0x003F},        // cam_stat_ae_initial_window_xend = 63
    { 0xC922,   2, 0x002F},        // cam_stat_ae_initial_window_yend = 47
    //============= End of regs marker ==================
    { 0x0000,   0, 0x0000}
};

static int host_command(sensor_t *sensor, uint16_t command)
{
    if (cambus_writew2(&sensor->bus, sensor->slv_addr, REG_HOST_COMMAND, (command | HC_OK)) != 0) {
        return -1;
    }

    for (mp_uint_t start = mp_hal_ticks_ms(); ;mp_hal_delay_ms(1)) {
        uint16_t reg_data;
        if (cambus_readw(&sensor->bus, sensor->slv_addr, REG_HOST_COMMAND, &reg_data) != 0) {
            return -1;
        }
        if ((reg_data & command) == 0) {
            return (reg_data & HC_OK) ? 0 : -1;
        }
        if ((mp_hal_ticks_ms() - start) >= CMD_TIMEOUT) {
            return -1;
        }
    }
    return 0;
}

static int set_system_state(sensor_t *sensor, uint8_t state)
{
    int ret = 0;
    ret |= cambus_writeb2(&sensor->bus, sensor->slv_addr, REG_SYSMGR_NEXT_STATE, state);
    return ret | host_command(sensor, HC_SET_STATE);
}

static uint8_t get_system_state(sensor_t *sensor)
{
    uint8_t reg_data;
    if (cambus_readb2(&sensor->bus, sensor->slv_addr, REG_SYSMGR_CURRENT_STATE, &reg_data) != 0) {
        return -1;
    }
    return reg_data;
}

static int reset(sensor_t *sensor)
{
    // TODO: Do we need to make sure it's suspended first ?
    if (get_system_state(sensor) != SS_SUSPENDED) {
        if (set_system_state(sensor, SS_ENTER_SUSPEND) != 0) {
            return -1;
        }
    }

    const uint32_t (*regs)[3] = qvga_24mhz_regs;
    for (int i=0, ret=0; regs[i][0]; i++) {
        uint16_t addr = __REV16(regs[i][0]);
        uint32_t size = regs[i][1]; 
        uint32_t data; 
        // TODO: could use indirect/logical addressing, please test it.
        switch (size) {
            case 4:
                data = __REV(regs[i][2]);
            case 2:
                data = __REV16(regs[i][2]);
            default:
                data = regs[i][2] & 0xff;
        }
        ret |= cambus_write_bytes(&sensor->bus, sensor->slv_addr, (uint8_t *) &addr, 2, CAMBUS_XFER_SUSPEND);
        ret |= cambus_write_bytes(&sensor->bus, sensor->slv_addr, (uint8_t *) &data, size, CAMBUS_XFER_NO_FLAGS);
        if (ret != 0) {
            return -2;
        }
    }

    if (cambus_writew2(&sensor->bus, sensor->slv_addr,
                REG_CAM_OUTPUT_FORMAT,
                (OUTPUT_FORMAT_RGB | OUTPUT_FORMAT_RGB565)) != 0) {
        return -3;
    }

    if (set_system_state(sensor, SS_ENTER_CONFIG_CHANGE) != 0) {
        return -4;
    }

    if (set_system_state(sensor, SS_START_STREAMING) != 0) {
        return -5;
    }

    return 0;
}

static int read_reg(sensor_t *sensor, uint16_t reg_addr)
{
    uint16_t reg_data;
    if (cambus_readw(&sensor->bus, sensor->slv_addr, reg_addr, &reg_data) != 0) {
        return -1;
    }
    return reg_data;
}

static int write_reg(sensor_t *sensor, uint16_t reg_addr, uint16_t reg_data)
{
    return cambus_writew2(&sensor->bus, sensor->slv_addr, reg_addr, reg_data);
}

int mt9m114_init(sensor_t *sensor)
{
    // Initialize sensor structure.
    sensor->gs_bpp              = 2;
    sensor->reset               = reset;
    sensor->read_reg            = read_reg;
    sensor->write_reg           = write_reg;

    // Set sensor flags
    SENSOR_HW_FLAGS_SET(sensor, SENSOR_HW_FLAGS_VSYNC, 0);
    SENSOR_HW_FLAGS_SET(sensor, SENSOR_HW_FLAGS_HSYNC, 0);
    SENSOR_HW_FLAGS_SET(sensor, SENSOR_HW_FLAGS_PIXCK, 0);
    SENSOR_HW_FLAGS_SET(sensor, SENSOR_HW_FLAGS_FSYNC, 0);
    SENSOR_HW_FLAGS_SET(sensor, SENSOR_HW_FLAGS_JPEGE, 0);
    SENSOR_HW_FLAGS_SET(sensor, SWNSOR_HW_FLAGS_RGB565_REV, 1);
    return 0;
}

#endif // (OMV_ENABLE_MT9M114 == 1)
